#include "init.h"	// included by C-Wiz
#include <pic.h>
//EWeek 2008 Lockheed Martin
// Peter Gebhard, Patrick Wheeler

//for Hi-Tech C only

#include "delay.h" //make sure that PIC_CLK in this file is correctly defined

#define PREAMBLE1_LOW_THRESH     50
#define PREAMBLE1_HIGH_THRESH    150
#define PREAMBLE0_LOW_THRESH     50
#define PREAMBLE0_HIGH_THRESH    150
#define PULSESHORTLOW 			 10
#define PULSELONGLOW			 0x12
#define PULSELONGHIGH			 0x40

unsigned int los[16] = {1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0};
//static volatile       unsigned char	PARSECOUNT		@ 0x110;
//static volatile       unsigned char	PARSECOUNT		@ 0x200 = 1;
//static volatile       unsigned char	PARSECOUNT2		@ 0x210 = 1;
int inputWord[2];

void motor(unsigned int mode, unsigned int msec)
{
	/*
	PORTB: pin 7:6 - Left motor  (7: reverse, 6: forward)  128 64
	       pin 5:4 - Right motor (5: reverse, 4: forward)  32 16
	Mode 80: Right and Left motors forward
	Mode 96: Right motor reverse, Left motor forward	
	Mode 144: Right motor forward, Left motor reverse
	Mode 160: Right and Left motors reverse
	*/
	PORTB = mode;	//Turn on designated motors
	DelayMs(msec);  //Keep motor(s) on for set time
	PORTB = 0x00;   //Turn off all motor outputs
}

void pwm(unsigned int pwmNum, unsigned int microsec)
{	
	//PWM 1: RC2, PWM 2: RC4
	
	//Setup (as per section 11.3.7 in datasheet)
	TRISC = 0xDF;
	PR2 = 0xFF;
	
	if(pwmNum == 1)
	{
		CCP1CON = 0xFE; //P1B modulated
	}
	if(pwmNum == 2)
	{
		CCP1CON = 0x8E; //P1B modulated
	}
	
	//Something here
	
}

//Labeled as leftTurret
unsigned int closeGripper(unsigned int position)
{
	//unsigned int adjusted = microsec - 200;
	
	///if(adjusted < 900) //Don't let the period drop below 1000microsec
	//{
	//	return microsec;
	//}
	
	//pwm(1, adjusted); //PWM 1, decrease PWM period 200 microsec (closes 20% more)
	RC3=1;
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	RC3=0;
	
	return position-1;
}

//Labeled as rightTurret
unsigned int openGripper(unsigned int position)
{	
	//unsigned int adjusted = microsec + 200;
	
	//if(adjusted > 2100) //Don't let the period increase above 2000microsec
	//{
	//	return microsec;
	//}
	
	//pwm(1, adjusted); //PWM 1, increase PWM period 200 microsec (opens 20% more)
	
	RC3=1;
	DelayUs(200);
	DelayUs(200);
	DelayUs(200);
	RC3=0;
	return position+1;
}

//Labeled as gun
unsigned int lowerGripper(unsigned int position)
{
	//unsigned int adjusted = microsec - 200;
	
	//if(adjusted < 900) //Don't let the period drop below 1000microsec
	//{
	//	return microsec;
	//}
	
	//pwm(2, adjusted); //PWM 2, decrease PWM period 200 microsec (lowers 20% more)
	PORTC=0b1010;
	DelayMs(100);
	PORTC=0;
	return position-1;
}

//Labeled as fire
unsigned int raiseGripper(unsigned int position)
{
	//unsigned int adjusted = microsec + 200;
	
	//if(adjusted > 2100) //Don't let the period increase above 2000microsec
	//{
	//	return microsec;
	//}
	
	//pwm(2, adjusted); //PWM 2, increase PWM period 200 microsec (lowers 20% more)
	PORTC=0b0101;
	DelayMs(100);
	PORTC=0;
	return position+1;
}

//Labeled as leftMotor
void left(void)
{
	PORTC= 0x3;
	DelayMs(100);
	PORTC = 0x0;
	//motor(144, 25); //Right motor forward, Left motor reverse for 100msec
}

//Labeled as rightMotor
void right(void)
{
	PORTC=0b1100;
	DelayMs(100);
	PORTC=0;
	//motor(96, 25); //Right motor reverse, Left motor forward for 100msec
}

void forward(void)
{
	//motor(80, 100); //Right and Left motors forward for 100msec
	RC1=1;
	DelayMs(100);
	RC1=0;
}

void reverse(void)
{
	//motor(160, 100); //Right and Left motors reverse for 100msec
	RC0=1;
	DelayMs(100);
	RC0=0;
}

void leftTrim(void)
{
	//Unused - available
}

void rightTrim(void)
{
	//Unused - available
}

void parseIR(void)
{
    //int current;
    int count;
    int i;
    unsigned int data[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//    PARSECOUNT = 0;
//    PARSECOUNT2 = 0;
 	//RC0=0;   
    while(1)
    {
    	for(i=0;i<16;i++)
    	{
    		data[i] = 0;
    	}
        //current = RA0;
        //RC1 = 0;
        if(RB5 == 1)
        {
			DelayUs(10);
    		//RC1 = 1;
			//DelayUs(50);
			//RC1 = 0;
            continue;
        }
        count = 0;
        while(RB5 == 0 )
        {
            count++;
			DelayUs(10);
			//RC2 = 1;
			//DelayUs(50);
			//RC2 = 0;
            //current = RA0;
        }
        if( (count < PREAMBLE0_LOW_THRESH) || (count > PREAMBLE0_HIGH_THRESH) )
        {
        	DelayUs(10);
            continue;
        }
        //RC2 = 1;
        count = 0;
        while(RB5 == 1)
        {
            count++;
			DelayUs(10);
            //current = RA0;
        }
        if( (count < PREAMBLE1_LOW_THRESH) || (count > PREAMBLE1_HIGH_THRESH) )
        {
    		//RC2 = 1;
	        //DelayS(1);
	        //RC2 = 0;
	        //DelayS(1);
	        //continue;
        	DelayUs(10);
            continue;
        }
        count = 0;
        for(i = 0; i<16; i++)
        {
        	while(RB5 == 0)
        	{
				DelayUs(10);
        	}
        	count = 0;
        	while(RB5 == 1)
        	{
        		count++;
        		if(count > PULSELONGHIGH)
        		{
        			break;
        		}
        		DelayUs(10);
        	}
        	if(count > PULSELONGHIGH)
    		{
    			break;
    		}
        	if(count > PULSELONGLOW)
        	{
        		data[i] = 1;
        	}
        	
        	//data[i] = count;
        }
        
        if(data[0] == 1)
        	continue;
        
        
        for(i=0;i<16;i++)
        {
        	eeprom_write(i*0x10, data[i]);
        }
        
        inputWord[0] = (data[0]<<7 | data[1]<<6 | data[2]<<5 | data[3]<<4 | data[4]<<3 | data[5]<<2 | data[6]<<1 | data[7]);
        inputWord[1] = (data[8]<<7 | data[9]<<6 | data[10]<<5 | data[11]<<4 | data[12]<<3 | data[13]<<2 | data[14]<<1 | data[15]);
        eeprom_write(0x15, inputWord[0]);
        eeprom_write(0x25, inputWord[1]);
        //RC3 = 1;
       // DelayS(2);
        //RC0=1;
        break;
        //RC1=0;
        //RC2=0;
        //RC3=0;
        //DelayS(1);
		//RC2 = 0;	

        
        
        //RC0 = data[0];
        //RC1 = data[1];
        //RC2 = data[2];
        //RC3 = data[3];
        //DelayS(2);
        //RC0 = 0;
        //RC1 = 0;
        //RC2 = 0;
        //RC3 = 0;
        //DelayS(1);
        //left(); //DEBUG - Test left turn
        //break;
    }
}

void main(void)
{

	unsigned int gripPosition = 0;
	unsigned int verticalPosition = 0;

	//gripPosition = 1000;
	//verticalPosition = 1000;
	
	//Setup chip

	//CCP1CON = 0x3D;   //PWM Config register
	//PEIE = 1;         //Turn on Peripheral Interrupts
	
	//int inputWord;
	init();	// Function call inserted by C-Wiz
	//RC0 = 1;
	//DelayS(5);
	//RC0 = 0;
	
    while(1)
	{
		//RC0 = 1;
//		DelayMs(125);
		//DelayS(1);
		//RC0 = 0;
//		DelayMs(125);
		//DelayS(1);
		
		parseIR();  //Infinite loop that interrupts when a word is properly parsed
		
		if(inputWord[0] == 0x20 && inputWord[1] == 0x41) //???? Close Gripper
		{
			gripPosition = closeGripper(gripPosition);
		}
		
		else if(inputWord[0] == 0x20 && inputWord[1] == 0x81) //???? Open Gripper
		{
			gripPosition = openGripper(gripPosition);
		}
		
		else if(inputWord[0] == 0x20 && inputWord[1] == 0x21) //???? Lower Gripper
		{
			verticalPosition = lowerGripper(verticalPosition);
		}
		
		else if(inputWord[0] == 0x20 && inputWord[1] == 0x11) //???? Raise Gripper
		{
			verticalPosition = raiseGripper(verticalPosition);
		}
		
		else if(inputWord[0] == 0x22 && inputWord[1] == 0x01) //???? Left
		{
			left();
		}
		
		else if(inputWord[0] == 0x21 && inputWord[1] == 0x01) //???? Right
		{
			right();
		}
		
		else if(inputWord[0] == 0x28 && inputWord[1] == 0x01) //???? Forward
		{
			forward();
		}
		
		else if(inputWord[0] == 0x24 && inputWord[1] == 0x01) //???? Reverse
		{
			reverse();
		}
	}
}
